"""
GomerX 夹爪控制模块
用于控制机器人夹爪的开合
"""

# 导入必要的系统模块
import time  # 时间模块，用于模拟夹爪动作延迟
from config.settings import GRIPPER_CONFIG  # 导入夹爪配置参数

class Gripper:
    """GomerX 夹爪控制类
    
    负责控制机器人夹爪的打开、关闭、释放和夹持力设置等操作
    提供夹爪状态查询功能，包括是否打开和是否持有物品
    """
    
    def __init__(self):
        """初始化夹爪控制器
        
        从配置文件加载夹爪参数，包括打开角度、关闭角度和默认夹持力
        """
        # 从配置文件加载夹爪参数
        self.open_angle = GRIPPER_CONFIG["open_angle"]  # 夹爪完全打开时的角度(度)
        self.close_angle = GRIPPER_CONFIG["close_angle"]  # 夹爪关闭时的角度(度)
        self.force = GRIPPER_CONFIG["force"]  # 默认夹持力
        
        # 夹爪状态变量
        self.is_connected = False  # 夹爪连接状态，初始为未连接
        self.is_open = True  # 夹爪开合状态，初始为打开状态
        self.is_holding = False  # 是否持有物品，初始为未持有
        
    def connect(self):
        """连接到夹爪
        
        初始化与夹爪硬件的通信连接
        
        Returns:
            bool: 连接是否成功
        """
        try:
            # 这里应该是连接到夹爪的实际代码
            # 例如：初始化串口通信、网络连接或硬件接口
            # 由于没有真实的SDK，这里只是模拟连接
            self.is_connected = True  # 更新连接状态
            print("夹爪连接成功")  # 输出连接成功信息
            return True  # 返回连接成功
        except Exception as e:
            # 捕获连接过程中的异常
            print(f"连接夹爪失败: {e}")  # 输出错误信息
            return False  # 返回连接失败
    
    def disconnect(self):
        """断开与夹爪的连接
        
        释放夹爪硬件资源，关闭通信连接
        """
        # 检查是否已连接
        if self.is_connected:
            # 这里应该是断开连接的实际代码
            # 例如：关闭串口、断开网络连接或释放硬件接口
            self.is_connected = False  # 更新连接状态
            print("夹爪连接已断开")  # 输出断开连接信息
    
    def open(self, angle=None):
        """
        打开夹爪
        
        控制夹爪打开到指定角度或默认角度
        
        Args:
            angle: 打开角度(度)，如果为None则使用默认打开角度
            
        Returns:
            bool: 打开是否成功
        """
        # 检查夹爪是否已连接
        if not self.is_connected:
            print("夹爪未连接")  # 输出错误信息
            return False  # 返回失败
            
        # 确定实际使用的打开角度：优先使用传入参数，否则使用默认角度
        open_angle = angle if angle is not None else self.open_angle
        
        try:
            # 这里应该是控制夹爪打开的实际代码
            # 例如：向夹爪发送打开指令，包含角度参数
            print(f"夹爪打开，角度: {open_angle}°")  # 输出调试信息
            
            # 模拟夹爪动作时间，实际硬件操作需要一定时间
            time.sleep(0.5)  # 模拟0.5秒的动作时间
            
            # 更新夹爪状态
            self.is_open = True  # 设置为打开状态
            self.is_holding = False  # 设置为未持有物品状态
            
            return True  # 返回成功
        except Exception as e:
            # 捕获控制过程中的异常
            print(f"打开夹爪失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def close(self, angle=None, force=None):
        """
        关闭夹爪
        
        控制夹爪关闭到指定角度或默认角度，并设置夹持力
        
        Args:
            angle: 关闭角度(度)，如果为None则使用默认关闭角度
            force: 夹持力，如果为None则使用默认夹持力
            
        Returns:
            bool: 关闭是否成功
        """
        # 检查夹爪是否已连接
        if not self.is_connected:
            print("夹爪未连接")  # 输出错误信息
            return False  # 返回失败
            
        # 确定实际使用的关闭角度：优先使用传入参数，否则使用默认角度
        close_angle = angle if angle is not None else self.close_angle
        
        # 确定实际使用的夹持力：优先使用传入参数，否则使用默认夹持力
        grip_force = force if force is not None else self.force
        
        try:
            # 这里应该是控制夹爪关闭的实际代码
            # 例如：向夹爪发送关闭指令，包含角度和夹持力参数
            print(f"夹爪关闭，角度: {close_angle}°，夹持力: {grip_force}")  # 输出调试信息
            
            # 模拟夹爪动作时间，实际硬件操作需要一定时间
            time.sleep(0.5)  # 模拟0.5秒的动作时间
            
            # 更新夹爪状态
            self.is_open = False  # 设置为关闭状态
            self.is_holding = True  # 设置为持有物品状态
            
            return True  # 返回成功
        except Exception as e:
            # 捕获控制过程中的异常
            print(f"关闭夹爪失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def release(self):
        """
        释放夹爪中的物品
        
        通过打开夹爪来释放当前持有的物品
        
        Returns:
            bool: 释放是否成功
        """
        # 直接调用open方法来释放物品
        return self.open()
    
    def is_gripper_open(self):
        """
        检查夹爪是否打开
        
        Returns:
            bool: 夹爪是否打开
        """
        return self.is_open  # 返回夹爪开合状态
    
    def is_holding_object(self):
        """
        检查夹爪是否持有物品
        
        Returns:
            bool: 是否持有物品
        """
        return self.is_holding  # 返回夹爪持有物品状态
    
    def set_gripper_force(self, force):
        """
        设置夹爪夹持力
        
        调整夹爪的夹持力大小，用于适应不同重量和材质的物品
        
        Args:
            force: 夹持力大小
            
        Returns:
            bool: 设置是否成功
        """
        # 检查夹爪是否已连接
        if not self.is_connected:
            print("夹爪未连接")  # 输出错误信息
            return False  # 返回失败
            
        try:
            # 这里应该是设置夹持力的实际代码
            # 例如：向夹爪发送设置夹持力的指令
            self.force = force  # 更新夹持力值
            print(f"夹爪夹持力设置为: {force}")  # 输出调试信息
            return True  # 返回成功
        except Exception as e:
            # 捕获设置过程中的异常
            print(f"设置夹爪夹持力失败: {e}")  # 输出错误信息
            return False  # 返回失败